// --------------------------------------------------------------------------------------------------------------------
// <copyright file="Vector4.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Numerics
{
    using System;
    using System.ComponentModel;

    /// <summary>
    /// Vector4 class
    /// </summary>
    [TypeConverter(typeof(Vector4Converter))]
    public struct Vector4
    {
        /// <summary>
        /// The x component
        /// </summary>
        private double xcomponent;

        /// <summary>
        /// The y component
        /// </summary>
        private double ycomponent;

        /// <summary>
        /// The z component
        /// </summary>
        private double zcomponent;

        /// <summary>
        /// The w component
        /// </summary>
        private double wcomponent;

        /// <summary>
        /// Initializes a new instance of the Vector4 struct.
        /// </summary>
        /// <param name="x">The x component</param>
        /// <param name="y">The y component</param>
        /// <param name="z">The z component</param>
        /// <param name="w">The w component</param>
        public Vector4(double x, double y, double z, double w)
        {
            this.xcomponent = x;
            this.ycomponent = y;
            this.zcomponent = z;
            this.wcomponent = w;
        }

        /// <summary>
        /// Initializes a new instance of the Vector4 struct.
        /// </summary>
        /// <param name="v">Vector to copy</param>
        public Vector4(Vector4 v)
        {
            this.xcomponent = v.xcomponent;
            this.ycomponent = v.ycomponent;
            this.zcomponent = v.zcomponent;
            this.wcomponent = v.wcomponent;
        }

        /// <summary>
        /// Initializes a new instance of the Vector4 struct.
        /// </summary>
        /// <param name="v">The 3 vector</param>
        public Vector4(Vector3 v)
        {
            this.xcomponent = v.X;
            this.ycomponent = v.Y;
            this.zcomponent = v.Z;
            this.wcomponent = 1.0;
        }

        /// <summary>
        /// Gets or sets x component
        /// </summary>
        [Description("The X component of the vector.")]
        public double X
        {
            get
            {
                return this.xcomponent;
            }

            set
            {
                this.xcomponent = value;
            }
        }

        /// <summary>
        /// Gets or sets y component
        /// </summary>
        [Description("The Y component of the vector.")]
        public double Y
        {
            get
            {
                return this.ycomponent;
            }

            set
            {
                this.ycomponent = value;
            }
        }

        /// <summary>
        /// Gets or sets z component
        /// </summary>
        [Description("The Z component of the vector.")]
        public double Z
        {
            get
            {
                return this.zcomponent;
            }

            set
            {
                this.zcomponent = value;
            }
        }

        /// <summary>
        /// Gets or sets w component
        /// </summary>
        [Description("The W component of the vector.")]
        public double W
        {
            get
            {
                return this.wcomponent;
            }

            set
            {
                this.wcomponent = value;
            }
        }

        /// <summary>
        /// Vector4 + Operator
        /// </summary>
        /// <param name="left">Left side</param>
        /// <param name="right">Right side</param>
        /// <returns>Vector sum</returns>
        public static Vector4 operator +(Vector4 left, Vector4 right)
        {
            return new Vector4(
                left.xcomponent + right.xcomponent, 
                left.ycomponent + right.ycomponent, 
                left.zcomponent + right.zcomponent, 
                left.wcomponent + right.wcomponent);
        }

        /// <summary>
        /// Vector4 * operator with matrix
        /// </summary>
        /// <param name="m">Matrix left side</param>
        /// <param name="v">Vector right side</param>
        /// <returns>Vector result</returns>
        public static Vector4 operator *(Matrix4 m, Vector4 v)
        {
            Vector4 result = new Vector4();
            result.xcomponent = (m[0, 0] * v.xcomponent) + (m[0, 1] * v.ycomponent) + (m[0, 2] * v.zcomponent)
                                + (m[0, 3] * v.wcomponent);
            result.ycomponent = (m[1, 0] * v.xcomponent) + (m[1, 1] * v.ycomponent) + (m[1, 2] * v.zcomponent)
                                + (m[1, 3] * v.wcomponent);
            result.zcomponent = (m[2, 0] * v.xcomponent) + (m[2, 1] * v.ycomponent) + (m[2, 2] * v.zcomponent)
                                + (m[2, 3] * v.wcomponent);
            result.wcomponent = (m[3, 0] * v.xcomponent) + (m[3, 1] * v.ycomponent) + (m[3, 2] * v.zcomponent)
                                + (m[3, 3] * v.wcomponent);

            return result;
        }

        /// <summary>
        /// Normalize a vector to a unit vector
        /// </summary>
        /// <param name="v">Vector to be normalized</param>
        /// <returns>Normalized vector</returns>
        public static Vector4 Normalize(Vector4 v)
        {
            double mag = v.Length();
            Vector4 unitv = new Vector4();

            // avoid division by 0
            if (mag != 0)
            {
                unitv.xcomponent = v.xcomponent / mag;
                unitv.ycomponent = v.ycomponent / mag;
                unitv.zcomponent = v.zcomponent / mag;
                unitv.wcomponent = v.wcomponent / mag;
            }
            else
            {
                unitv.xcomponent = v.xcomponent;
                unitv.ycomponent = v.ycomponent;
                unitv.zcomponent = v.zcomponent;
                unitv.wcomponent = v.wcomponent;
            }

            return unitv;
        }

        /// <summary>
        /// Compute the L2 norm of a vector
        /// </summary>
        /// <param name="v">A vector</param>
        /// <returns>Norm of the vector</returns>
        public static double ComputeL2Norm(Vector4 v)
        {
            return Math.Sqrt(ComputeSquaredL2Norm(v));
        }

        /// <summary>
        /// Compute the square of the L2 norm of a vector
        /// </summary>
        /// <param name="v">A vector</param>
        /// <returns>Squared norm of the vector</returns>
        public static double ComputeSquaredL2Norm(Vector4 v)
        {
            return v.X * v.X + v.Y * v.Y + v.Z * v.Z + v.W * v.W;
        }

        /// <summary>
        /// Returns a vector pointing in the opposite direction.
        /// </summary>
        /// <param name="value">Source vector.</param>
        /// <returns>Vector pointing in the opposite direction.</returns>
        public static Vector4 operator -(Vector4 value)
        {
            return new Vector4(-value.xcomponent, -value.ycomponent, -value.zcomponent, -value.wcomponent);
        }

        /// <summary>
        /// Vector4 operator -
        /// </summary>
        /// <param name="left">Left side</param>
        /// <param name="right">Right side</param>
        /// <returns>The vector result</returns>
        public static Vector4 operator -(Vector4 left, Vector4 right)
        {
            return new Vector4(
                left.xcomponent - right.xcomponent,
                left.ycomponent - right.ycomponent,
                left.zcomponent - right.zcomponent,
                left.wcomponent - right.wcomponent);
        }

        /// <summary>
        /// Vector4 operator /
        /// </summary>
        /// <param name="v">Vector divisor</param>
        /// <param name="value">Value to divide by</param>
        /// <returns>The vector result</returns>
        public static Vector4 operator /(Vector4 v, double value)
        {
            return new Vector4(v.xcomponent / value, v.ycomponent / value, v.zcomponent / value, v.wcomponent / value);
        }

        /// <summary>
        /// Vector4 operator *
        /// </summary>
        /// <param name="v">Vector to multiply</param>
        /// <param name="value">Value to multiply by</param>
        /// <returns>The vector result</returns>
        public static Vector4 operator *(Vector4 v, double value)
        {
            return new Vector4(v.xcomponent * value, v.ycomponent * value, v.zcomponent * value, v.wcomponent * value);
        }

        /// <summary>
        /// Vector4 operator *
        /// </summary>
        /// <param name="value">Value to multiply by</param>
        /// <param name="v">Vector to multiply</param>
        /// <returns>The vector result</returns>
        public static Vector4 operator *(double value, Vector4 v)
        {
            return new Vector4(v.xcomponent * value, v.ycomponent * value, v.zcomponent * value, v.wcomponent * value);
        }

        /// <summary>
        /// Dot product
        /// </summary>
        /// <param name="left">Left side</param>
        /// <param name="right">Right side</param>
        /// <returns>Scalar result</returns>
        public static double Dot(Vector4 left, Vector4 right)
        {
            return (left.xcomponent * right.xcomponent) + (left.ycomponent * right.ycomponent)
                   + (left.zcomponent * right.zcomponent)
                   + (left.wcomponent * right.wcomponent);
        }

        /// <summary>
        /// Get the 3 vector
        /// </summary>
        /// <returns>The first three components</returns>
        public Vector3 GetVector3()
        {
            return new Vector3(this.xcomponent, this.ycomponent, this.zcomponent);
        }

        /// <summary>
        /// From a 3 vector
        /// </summary>
        /// <param name="v">Vector to copy</param>
        public void SetFromVector3(Vector3 v)
        {
            this.xcomponent = v.X;
            this.ycomponent = v.Y;
            this.zcomponent = v.Z;
            this.wcomponent = 1.0;
        }

        /// <summary>
        /// Get the length of the vector
        /// </summary>
        /// <returns>Vector length</returns>
        public double Length()
        {
            return
                Math.Sqrt(
                    (this.xcomponent * this.xcomponent) + (this.ycomponent * this.ycomponent)
                    + (this.zcomponent * this.zcomponent) + (this.wcomponent * this.wcomponent));
        }

        /// <summary>
        /// Retrieves a string representation of the Vector4.
        /// </summary>
        /// <returns>String that represents the Vector4.</returns>
        public override string ToString()
        {
            return string.Format("X:{0} Y:{1} Z:{2} W:{3}", this.X, this.Y, this.Z, this.W);
        }
    }
}
